#include "udp_gps_forwarder/UavUdpComm.hpp"

UavUdpComm::UavUdpComm() : Node("uav_udp_comm") {


    this->declare_parameter<std::string>("namespace", "/drone");
    this->declare_parameter<int>("vehicle_id", 0);

    this->get_parameter("namesspace", namespaces);
    RCLCPP_INFO(this->get_logger(), "Namespace: %s", namespaces.c_str());

    if (this->get_parameter("vehicle_id", vehcile_id_)) {
        RCLCPP_INFO(this->get_logger(), "Vehicle IDs: %d", vehcile_id_);
    } else {
        RCLCPP_WARN(this->get_logger(), "Parameter 'vehicle_ids' not found");
    }
    namespaces = namespaces + "_" + std::to_string(vehcile_id_);
  
//     this->declare_parameter<std::string>("parameters.namesspace", "drone_12" );
//     this->declare_parameter<int>("parameters.vehicle_id", 1 );



//   // ----------------------------------------reader parameters from config file----------------------------------------
//   namespaces = this->get_parameter("parameters.namesspace").as_string();
//   vehcile_id_ = this->get_parameter("parameters.vehicle_id").as_int();
  
    RCLCPP_INFO(this->get_logger(),"parameters.namesspace: %s" , namespaces.c_str());
    RCLCPP_INFO(this->get_logger(),"parameters.vehicle_id: %d",vehcile_id_);
    RCLCPP_INFO(this->get_logger(), "初始化节点");

    // 订阅GPS数据
    gps_subscriber_ = this->create_subscription<sensor_msgs::msg::NavSatFix>(
        namespaces + "/global_position/raw/fix", rclcpp::QoS(10).best_effort(),
        std::bind(&UavUdpComm::gps_callback, this, std::placeholders::_1));
    // 订阅IMU数据
    imu_subscriber_ = this->create_subscription<sensor_msgs::msg::Imu>(
        namespaces + "/imu/data",  rclcpp::QoS(10).best_effort(),
        std::bind(&UavUdpComm::imu_callback, this, std::placeholders::_1));

    // 订阅当前的无人机状态
    state_sub_ = this->create_subscription<mavros_msgs::msg::State>(
        namespaces +"/state", rclcpp::QoS(10).best_effort(), 
        std::bind(&UavUdpComm::state_callback, this, std::placeholders::_1));

    // 发布地理位置
    pose_publisher_ = this->create_publisher<geographic_msgs::msg::GeoPoseStamped>(
        namespaces + "/setpoint_position/global", 10);
    // 发布gps的位置控制信息
    std::string yolo_conroller = uav_namespaces + namespaces + "/gps_move";
    yolo_publisher_ = this->create_publisher<yolo_detect_message::msg::Geolocation>(uav_namespaces + namespaces + "/gps_move", 10);
    RCLCPP_INFO(this->get_logger(),"uav_namespaces + namespaces + /gps_move:%s",yolo_conroller.c_str());
    arm_client_ = this->create_client<mavros_msgs::srv::CommandBool>(
            namespaces + "/cmd/arming");
    
    setup_udp_communication();

    init_gps_received = false;
    init_imu_received = false;
    // 设置模式的服务客户端
    set_mode_client_ = this->create_client<mavros_msgs::srv::SetMode>(namespaces + "/set_mode");
    last_request_ = this->get_clock()->now();
    // 启动UDP通信线程
    receive_thread_ = std::thread(&UavUdpComm::udp_receive_thread, this);

    send_thread_ = std::thread(&UavUdpComm::udp_send_thread, this);
}

UavUdpComm::~UavUdpComm() {
    if (receive_thread_.joinable()) {
        receive_thread_.join();
    }
    if (send_thread_.joinable()) {
        send_thread_.join();
    }
}

static bool* setOffboardMode() {
    auto set_mode_client_ = this->create_client<mavros_msgs::srv::SetMode>("/drone_0/set_mode");
    auto request = std::make_shared<mavros_msgs::srv::SetMode::Request>();
    request->custom_mode = "OFFBOARD";

    RCLCPP_INFO(this->get_logger(), "Attempting to set Offboard mode...");

    // 发送异步请求
    auto result_future = set_mode_client_->async_send_request(request, [this](rclcpp::Client<mavros_msgs::srv::SetMode>::SharedFuture future) {
        if (future.get()->mode_sent) {
            RCLCPP_INFO(this->get_logger(), "Offboard mode set successfully.");
        } else {
            RCLCPP_ERROR(this->get_logger(), "Failed to set Offboard mode.");
            return false; //返回false表示设置offboard模式失败
        }
    });
    return true; // 返回true仅表示请求已发送
}

static bool* set1ffboardMode() {
    auto set_mode_client_ = this->create_client<mavros_msgs::srv::SetMode>("/drone_1/set_mode");
    auto request = std::make_shared<mavros_msgs::srv::SetMode::Request>();
    request->custom_mode = "OFFBOARD";

    RCLCPP_INFO(this->get_logger(), "Attempting to set Offboard mode...");

    // 发送异步请求
    auto result_future = set_mode_client_->async_send_request(request, [this](rclcpp::Client<mavros_msgs::srv::SetMode>::SharedFuture future) {
        if (future.get()->mode_sent) {
            RCLCPP_INFO(this->get_logger(), "Offboard mode set successfully.");
        } else {
            RCLCPP_ERROR(this->get_logger(), "Failed to set Offboard mode.");
            return false; //返回false表示设置offboard模式失败
        }
    });
    return true; // 返回true仅表示请求已发送
}

static bool* set2ffboardMode() {
    auto set_mode_client_ = this->create_client<mavros_msgs::srv::SetMode>("/drone_2/set_mode");
    auto request = std::make_shared<mavros_msgs::srv::SetMode::Request>();
    request->custom_mode = "OFFBOARD";

    RCLCPP_INFO(this->get_logger(), "Attempting to set Offboard mode...");

    // 发送异步请求
    auto result_future = set_mode_client_->async_send_request(request, [this](rclcpp::Client<mavros_msgs::srv::SetMode>::SharedFuture future) {
        if (future.get()->mode_sent) {
            RCLCPP_INFO(this->get_logger(), "Offboard mode set successfully.");
        } else {
            RCLCPP_ERROR(this->get_logger(), "Failed to set Offboard mode.");
            return false; //返回false表示设置offboard模式失败
        }
    });
    return true; // 返回true仅表示请求已发送
}

void UavUdpComm::gps_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg) {
    init_gps_received = true;
    //  RCLCPP_INFO(this->get_logger(), "进入GPS回调函数");
    // 处理接收到的GPS数据
    std::lock_guard<std::mutex> lock(data_mutex);
    // 更新GPS数据
    data.latitude = static_cast<int32_t>(msg->latitude * (1LL << 31) / 90.0);
    data.longitude = static_cast<int32_t>(msg->longitude * (1LL << 31) / 180.0);
    data.altitude = static_cast<int32_t>(msg->altitude * (1LL << 31) / 35000.0);
    //zsj调试
    std::cout<<"la"<<data.latitude;
    std::cout<<"long"<<data.longitude;
    std::cout<<"alt"<<data.altitude;
    // RCLCPP_INFO(this->get_logger(), "Received GPS data: Lat %f, Lon %f", msg->latitude, msg->longitude);
    
}

// IMU回调函数
void UavUdpComm::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) {
    init_imu_received = true;
    //  RCLCPP_INFO(this->get_logger(), "进入GPS回调函数");
    std::lock_guard<std::mutex> lock(data_mutex);
    // 更新IMU数据，并计算欧拉角
    double q_w = msg->orientation.w;
    double q_x = msg->orientation.x;
    double q_y = msg->orientation.y;
    double q_z = msg->orientation.z;
    double roll = atan2(2.0 * (q_w * q_x + q_y * q_z), 1.0 - 2.0 * (q_x * q_x + q_y * q_y));
    double pitch = asin(2.0 * (q_w * q_y - q_z * q_x));
    double yaw = atan2(2.0 * (q_w * q_z + q_x * q_y), 1.0 - 2.0 * (q_y * q_y + q_z * q_z));
    data.roll = static_cast<int16_t>(roll * 180.0 / M_PI * 100.0);
    data.pitch = static_cast<int16_t>(pitch * 180.0 / M_PI * 100.0);
    data.yaw = static_cast<int16_t>(yaw * 180.0 / M_PI * 100.0);
    // RCLCPP_INFO(this->get_logger(), "Received IMU data: Orientation [%f, %f, %f, %f]",
    //             msg->orientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w);
}

void UavUdpComm::udp_receive_thread() {
    FCUCMD cmd;
    bool offboard_set = false;  // Tracks whether Offboard mode has been enabled
    RCLCPP_INFO(this->get_logger(), "UDP Receive Thread Started");
    receiveSockfd = socket(AF_INET, SOCK_DGRAM, 0);
    if (receiveSockfd == -1) {
        RCLCPP_ERROR(this->get_logger(), "Failed to create UDP receive socket: %s", std::strerror(errno));
        return;
    }

    receiveAddr.sin_family = AF_INET;
    receiveAddr.sin_port = htons(UDP_RECEIVE_PORT);
    receiveAddr.sin_addr.s_addr = htonl(INADDR_ANY); // or specific interface
    if (bind(receiveSockfd, (struct sockaddr *)&receiveAddr, sizeof(receiveAddr)) < 0) {
        RCLCPP_ERROR(this->get_logger(), "Failed to bind UDP receive socket: %s", std::strerror(errno));
        close(receiveSockfd);
        return;
    }
    while (rclcpp::ok()) {
        int bytesReceived = recvfrom(receiveSockfd, &cmd, sizeof(FCUCMD), 0, reinterpret_cast<sockaddr*>(&receiveAddr), &addrLen);
        last_request_ = this->get_clock()->now();
        if (bytesReceived > 0) {
            if (bytesReceived == sizeof(FCUCMD)) {
                // RCLCPP_INFO(this->get_logger(), "Received FCUCMD: Setting Offboard");
                // if (!offboard_set) {
                //     offboard_set = setOffboardMode();
                // }
                // // 保证当前状态为OFFBOARD
                // if (current_state_.mode != "OFFBOARD" && (this->get_clock()->now() - last_request_ > rclcpp::Duration(5.0)))
                // {
                //     RCLCPP_WARN(this->get_logger(), "call mode to OFFBOARD...");
                //     set_offboard_mode();
                //     last_request_ = this->get_clock()->now();                    
                // }else if (!current_state_.armed && (this->get_clock()->now() - last_request_ > rclcpp::Duration(5.0)))
                // {
                //     RCLCPP_WARN(this->get_logger(), "Call Arming service ...");
                //     arm_drone();
                //     last_request_ = this->get_clock()->now();
                // }

                std::lock_guard<std::mutex> lock(udp_mutex);
                geographic_msgs::msg::GeoPoseStamped targetPose;
                targetPose.header.stamp = this->now();
                targetPose.pose.position.latitude = cmd.RefLatitude_deg;
                targetPose.pose.position.longitude = cmd.RefLongitude_deg;
                targetPose.pose.position.altitude = cmd.RefHeight_meter;
                // RCLCPP_ERROR(this->get_logger(), "UDP Receive error: %f", cmd.RefLatitude_deg);
                // RCLCPP_ERROR(this->get_logger(), "UDP Receive error: %f", cmd.RefLongitude_deg);
                // RCLCPP_ERROR(this->get_logger(), "UDP Receive error: %f", cmd.RefHeight_meter);
                // cmd.RefHeight_meter = 635;
                yolo_detect_message::msg::Geolocation geo_msg;
                geo_msg.latitude = cmd.RefLatitude_deg;
                geo_msg.longtitude = cmd.RefLongitude_deg;
                geo_msg.altitude = cmd.RefHeight_meter;
                yolo_publisher_->publish(geo_msg);
                // pose_publisher_->publish(targetPose);
                RCLCPP_INFO(this->get_logger(), "Set success");
            } else {
                RCLCPP_ERROR(this->get_logger(), "Incomplete FCUCMD data packet received");
            }
        } else if (bytesReceived < 0) {
            RCLCPP_ERROR(this->get_logger(), "UDP Receive error: %s", std::strerror(errno));
        }
    }
}
void UavUdpComm::setup_udp_communication() {
    sockfd_ = socket(AF_INET, SOCK_DGRAM, 0);
    if (sockfd_ < 0) {
        RCLCPP_ERROR(this->get_logger(), "Failed to create UDP socket");
        return;
    }
    //------设置套接字选项允许广播
    int broadcast_enable = 1;
    if(setsockopt(sockfd_,SOL_SOCKET,SO_BROADCAST,&broadcast_enable,sizeof(broadcast_enable))<0){
        RCLCPP_ERROR(this->get_logger(), "Failed to setsockopt");
        close(sockfd_);
        return;
    }
    //------
    
    memset(&destAddr_, 0, sizeof(destAddr_));
    destAddr_.sin_family = AF_INET;
    // destAddr_.sin_addr.s_addr = inet_addr("192.168.16.255");
    destAddr_.sin_port = htons(UDP_PORT);
    if (inet_pton(AF_INET, UDP_TARGET_IP, &destAddr_.sin_addr)  <= 0)
    {
        RCLCPP_INFO(this->get_logger(), "Invalid address");
        close(sockfd_);
    }
    
    // inet_pton(AF_INET, UDP_TARGET_IP, &destAddr_.sin_addr);
}

void UavUdpComm::start_udp_send_thread() {
    send_thread_ = std::thread(&UavUdpComm::udp_send_thread, this);
}

void UavUdpComm::udp_send_thread() {
    while (rclcpp::ok()) {
        if (init_gps_received == true||init_imu_received == true)
        {
             send_udp_data();
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(100)); // 发送间隔
    }
}

void UavUdpComm::send_udp_data() {
    
    std::lock_guard<std::mutex> lock(data_mutex_);
    ssize_t bytesSent = sendto(sockfd_, &data, sizeof(data), 0,
                                (struct sockaddr*)&destAddr_, sizeof(destAddr_));
    if (bytesSent < 0) {
        RCLCPP_ERROR(this->get_logger(), "Failed to send UDP data: %s", strerror(errno));
    } else {
        RCLCPP_INFO(this->get_logger(), "Successfully sent UDP data");
    }
}
void UavUdpComm::state_callback(const mavros_msgs::msg::State::SharedPtr msg){
    current_state_ = *msg;
    RCLCPP_INFO(this->get_logger(),"this get  message");

}

 void UavUdpComm::arm_drone()
    {
        if (!arm_client_->wait_for_service(std::chrono::seconds(1)))
        {
            RCLCPP_ERROR(this->get_logger(), "Arming service not available");
            return;
        }

        auto request = std::make_shared<mavros_msgs::srv::CommandBool::Request>();
        request->value = true;  // 发送ARM命令

        // 异步发送请求，并使用回调函数处理结果
        auto result_future = arm_client_->async_send_request(request,
            [this](rclcpp::Client<mavros_msgs::srv::CommandBool>::SharedFuture result) {
                if (result.get()->success)
                {
                    RCLCPP_INFO(this->get_logger(), "Drone armed successfully");
                }
                else
                {
                    RCLCPP_ERROR(this->get_logger(), "Failed to arm drone");
                }
            });
    }

    void UavUdpComm::set_offboard_mode()
    {
        if (!set_mode_client_->wait_for_service(std::chrono::seconds(1)))
        {
            RCLCPP_ERROR(this->get_logger(), "Set mode service not available");
            return;
        }

        auto request = std::make_shared<mavros_msgs::srv::SetMode::Request>();
        request->custom_mode = "OFFBOARD";

        // 异步发送请求，并使用回调函数处理结果
        auto result_future = set_mode_client_->async_send_request(request,
            [this](rclcpp::Client<mavros_msgs::srv::SetMode>::SharedFuture result) {
                if (result.get()->mode_sent)
                {
                    RCLCPP_INFO(this->get_logger(), "OFFBOARD mode enabled");
                }
                else
                {
                    RCLCPP_ERROR(this->get_logger(), "Failed to enable OFFBOARD mode");
                }
            });
    }
   


